#include <MPU6050_tockn.h>
#include "MyUDP.h"

MyUDP myUDP;
MPU6050 mpu6050(Wire);

float balanceX=0.2, balanceY=0.7;
float xp = 6.0, xi =0.1, xd =2.0;
float yp = 6.0, yi =0.1, yd =2.0;

int pwmX,pwmY,pwmF,pwmB,pwmL,pwmR;
float angleX,angleY,gyroX,gyroY;
float biasX,biasY;
float integrateX,integrateY;
int pwmXPID,pwmYPID;

String info="";
float gas=0.0;
int speed=0;
bool check=false;

void do_motors(int forward,int backward, int left, int right){
  //analogWrite(32, forward);
  //analogWrite(26, backward);
  analogWrite(27, left);
  analogWrite(33, right);
}

void pwm_do(){
  mpu6050.update();
  angleX = mpu6050.getAngleX();
  angleY = mpu6050.getAngleY();
  gyroX = mpu6050.getGyroX();
  gyroY = mpu6050.getGyroY();

  if(gas==0){
    do_motors(0, 0, 0, 0);
    return;
  }

  biasX = angleX - balanceX;
  biasY = angleY - balanceY;

  integrateX += biasX;
  integrateY += biasY;

  integrateX=constrain(integrateX, -500, 500);
  integrateY=constrain(integrateY, -500, 500);

  pwmX = 255 * gas;
  pwmY = 255 * gas;

  pwmXPID=xp * biasX + xi * integrateX + xd * gyroX;
  pwmYPID=yp * biasY + yi * integrateY + yd * gyroY;

  pwmXPID = pwmXPID / 2;
  pwmYPID = pwmYPID / 2;

  pwmXPID=constrain(pwmXPID, -100, 100);
  pwmYPID=constrain(pwmYPID, -100, 100);

  pwmF = pwmX + pwmXPID;
  pwmB = pwmX - pwmXPID;
  pwmL = pwmY + pwmYPID;
  pwmR = pwmY - pwmYPID;

  pwmF = constrain(pwmF, 0, 255);
  pwmB = constrain(pwmB, 0, 255);
  pwmL = constrain(pwmL, 0, 255);
  pwmR = constrain(pwmR, 0, 255);

  do_motors(pwmF, pwmB, pwmL, pwmR);
}

void changeSpeed(const char *s){
  switch (*s){
    case 'F':
      gas += 0.1;
      if(gas>1.0){
        gas = 1.0;
      }
      break;
    case 'B':
      gas -= 0.1;
      if(gas<0){
        gas=0;
      }
      break;
    case 'L':
      gas=0.5;
      break;
    case 'R':
      gas=0;
      break;
    case '0':
      gas=0;
      balanceX=0.2;
      balanceY=0.7;
      break;
    case '1':
      check=true;
      break;
    case '2':
      //xp+=1;
      yp+=1;
      break;
    case '3':
      //xp-=1;
      yp-=1;
      break;
    case '4':
      //xi+=0.1;
      yi+=0.1;
      break;
    case '5':
      //xi-=0.1;
      yi-=0.1;
      break;
    case '6':
      //xd+=0.1;
      yd+=0.1;
      break;
    case '7':
      //xd-=0.1;
      yd-=0.1;
      break;
    case 'c':
      //balanceX +=10;
      balanceY +=10;
      break;
    case 'd':
      //balanceX -=10;
      balanceY -=10;
      break;
  }
  gas = constrain(gas, 0.0, 1.0);
  //myUDP.sendData("gas:"+String(gas)+" xp:"+String(xp)+" xi:"+String(xi)+" xd:"+String(xd)+" balanceX:"+String(balanceX));
  myUDP.sendData("gas:"+String(gas)+" yp:"+String(yp)+" yi:"+String(yi)+" yd:"+String(yd)+" balanceY:"+String(balanceY));
}

void setup() {
  Serial.begin(9600);

  myUDP=MyUDP();
  myUDP.createWiFi("ESP32_WIFI","12345678", 3001);
  
  Wire.begin(22,23);
  mpu6050.begin();
  //mpu6050.calcGyroOffsets(true);
  mpu6050.setGyroOffsets(1.05,-0.76,0.83);
}

void loop() {
  pwm_do();
  info=myUDP.getData();
  if(info!=""){
    changeSpeed(info.c_str());
  }
  if(check){
    check=false;
    angleX = angleX-balanceX;
    angleY = angleY-balanceY;
    myUDP.sendData("X:"+String(angleX)+" Y:"+String(angleY));
  }
}
